#include <iostream>
#include "rviz_dis.hpp"
#include "pavel_valtr.hpp"
#include "sdqp/sdqp.hpp"
#include <geometry_msgs/PoseStamped.h>

Eigen::Vector2d robot_center;
void CallBack(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    robot_center = Eigen::Vector2d(msg->pose.position.x, msg->pose.position.y);
    std::cout << "robot_center_ has a value: " << robot_center.transpose() << std::endl;
    return;
}

// 计算点到线段的距离以及交点
double pointToSegmentDistance(const Eigen::Vector2d& P, 
                              const Eigen::Vector2d& A, const Eigen::Vector2d& B,
                              Eigen::Vector2d& closestPoint) 
{
    Eigen::Vector2d AB = B - A;
    Eigen::Vector2d AP = P - A;

    // 向量投影的系数 t
    double t = AP.dot(AB) / AB.squaredNorm();

    // 确定投影点的位置
    if (t < 0.0) {
        closestPoint = A;  // 投影点在线段外，最接近点为 A
    } else if (t > 1.0) {
        closestPoint = B;  // 投影点在线段外，最接近点为 B
    } else {
        closestPoint = A + t * AB;  // 投影点在边上
    }

    // 返回点到线段的距离
    return (P - closestPoint).norm();
}

// 计算点到多边形所有边的最短距离及交点
void findClosestEdgeAndDistance(const Eigen::MatrixXd& poly1, 
                                const Eigen::Vector2d& robot_center, 
                                Eigen::Vector2d& closestPoint, double& minDistance) 
{
    int numVertices = poly1.rows();
    minDistance = std::numeric_limits<double>::infinity(); // 初始设为无穷大

    // 遍历每条边，计算点到每条边的最短距离
    for (int i = 0; i < numVertices; ++i) {
        Eigen::Vector2d A = poly1.row(i);
        Eigen::Vector2d B = poly1.row((i + 1) % numVertices); // 注意多边形的最后一个顶点和第一个顶点相连

        Eigen::Vector2d currentClosestPoint;
        double currentDistance = pointToSegmentDistance(robot_center, A, B, currentClosestPoint);

        if (currentDistance < minDistance) {
            minDistance = currentDistance;
            closestPoint = currentClosestPoint;  // 更新最短距离和交点
        }
    }
    return;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "polyhedral_obstacles");
    ros::NodeHandle nh;

    // initialize a polygon
    rviz1DisSimp rd0(nh, "array", "map", "test_obs");

    Mat poly1;
    randPoly rp;
    rp.generate(8, 5.6, poly1);
    std::cout << "poly1:\n " << poly1 << std::endl;

    robot_center.setZero();
    ros::Subscriber robot_sub = nh.subscribe("/move_base_simple/goal", 1, CallBack);

    rd0.add_convex_poly_flat(0, poly1, 0.1);
    ros::Rate r(50);    // 延迟20ms
    while (ros::ok()) 
    {
        ros::spinOnce();
        if (!robot_center.isZero()) {
            rd0.add_point_sphere(2, robot_center);

            // 2-Dimensional QP Pronlem, formulate the constraints
            Eigen::Vector2d z = Eigen::Vector2d::Zero();
            Eigen::Vector2d c = Eigen::Vector2d::Zero();
            Eigen::Matrix2d Q = Eigen::Matrix2d::Identity();

            Eigen::MatrixXd A;
            A.resize(poly1.rows(), 2);
            for (int i = 0; i < poly1.rows(); ++i) {
                Eigen::Vector2d rowVector = poly1.row(i);
                A.row(i) = (robot_center - rowVector).transpose();
            }
            Eigen::VectorXd b = -1.0 * Eigen::VectorXd::Ones(poly1.rows());

            double cost = sdqp::sdqp<2>(Q, c, A, b, z);

            Eigen::Vector2d opt_x;
            if (cost != INFINITY) {
                opt_x = z / z.squaredNorm() + robot_center;
            }
            else {
                ROS_WARN("SDQP has not feasiable solution. Robot collided with an obs!");

                double minDistance;
                findClosestEdgeAndDistance(poly1, robot_center, opt_x, minDistance); // 计算最短距离和交点

                std::cout << "The closest point is: (" << opt_x.x() << ", " << opt_x.y() << ")" << std::endl;
                std::cout << "The minimum distance is: " << minDistance << std::endl;
            }

            // ros::Time::now().toNSec()
            rd0.add_point_sphere(3, opt_x);
            rd0.add_arrow(1, robot_center.x(), robot_center.y(), 0.15, opt_x.x(), opt_x.y(), 0.15, 0.2);

            robot_center.setZero();
        }

        rd0.send();
        r.sleep();
    }
    std::cout << "==============end============" << std::endl;

    return 0;
}



